#include "QuadTree.h"

bool Rect::intersect(const Segment& seg) const {
    int minX = min(seg.a.x, seg.b.x);
    int maxX = max(seg.a.x, seg.b.x);
    int minY = min(seg.a.y, seg.b.y);
    int maxY = max(seg.a.y, seg.b.y);
    return !(maxX < x0 || minX > x1 || maxY < y0 || minY > y1);
}

QuadNode::QuadNode(Rect r) : rect(r), is_leaf(true) {}

QuadNode::~QuadNode() {
    for (auto child : children) {
        delete child;
    }
}

QuadTree::QuadTree() : root(new QuadNode(Rect{ 0, 0, 65535, 65535 })), max_depth(8), max_segs_per_leaf(40) {}

QuadTree::~QuadTree() {
    delete root;
}

void QuadTree::split(QuadNode* node, int depth) {
    int x0 = node->rect.x0, y0 = node->rect.y0;
    int x1 = node->rect.x1, y1 = node->rect.y1;
    int midx = x0 + (x1 - x0) / 2;
    int midy = y0 + (y1 - y0) / 2;

    node->children = {
        new QuadNode(Rect{x0, y0, midx, midy}),
        new QuadNode(Rect{midx, y0, x1, midy}),
        new QuadNode(Rect{x0, midy, midx, y1}),
        new QuadNode(Rect{midx, midy, x1, y1})
    };
    node->is_leaf = false;

    vector<int> segs_to_keep;
    for (int seg_index : node->segs) {
        Segment& s = segments[seg_index];
        int minx = min(s.a.x, s.b.x);
        int maxx = max(s.a.x, s.b.x);
        int miny = min(s.a.y, s.b.y);
        int maxy = max(s.a.y, s.b.y);

        if (minx <= node->rect.x0 && maxx >= node->rect.x1 &&
            miny <= node->rect.y0 && maxy >= node->rect.y1) {
            segs_to_keep.push_back(seg_index);
        }
        else {
            for (auto child : node->children) {
                if (child->rect.intersect(s)) {
                    insert_into_node(seg_index, depth + 1, child);
                }
            }
            seg_to_nodes[seg_index].erase(node);
        }
    }
    node->segs = segs_to_keep;
}

void QuadTree::insert_into_node(int seg_index, int depth, QuadNode* node) {
    if (node->is_leaf) {
        if (node->segs.size() >= max_segs_per_leaf && depth < max_depth) {
            split(node, depth);
            insert_into_node(seg_index, depth, node);
            return;
        }
        else {
            node->segs.push_back(seg_index);
            seg_to_nodes[seg_index].insert(node);
        }
    }
    else {
        Segment& s = segments[seg_index];
        int minx = min(s.a.x, s.b.x);
        int maxx = max(s.a.x, s.b.x);
        int miny = min(s.a.y, s.b.y);
        int maxy = max(s.a.y, s.b.y);

        if (minx <= node->rect.x0 && maxx >= node->rect.x1 &&
            miny <= node->rect.y0 && maxy >= node->rect.y1) {
            node->segs.push_back(seg_index);
            seg_to_nodes[seg_index].insert(node);
        }
        else {
            for (auto child : node->children) {
                if (child->rect.intersect(s)) {
                    insert_into_node(seg_index, depth + 1, child);
                }
            }
        }
    }
}

double QuadTree::calc_dist_rect_sq(int x, int y, const Rect& rect) const {
    double dx = 0, dy = 0;
    if (x < rect.x0) dx = rect.x0 - x;
    else if (x > rect.x1) dx = x - rect.x1;
    if (y < rect.y0) dy = rect.y0 - y;
    else if (y > rect.y1) dy = y - rect.y1;
    return dx * dx + dy * dy;
}

double QuadTree::calc_dist_sq(int x, int y, const Segment& seg) const {
    const Point& A = seg.a;
    double apx = x - A.x;
    double apy = y - A.y;

    if (seg.len2 == 0) {
        return apx * apx + apy * apy;
    }

    double dot = apx * seg.dx + apy * seg.dy;
    double t = dot / seg.len2;

    if (t < 0.0) {
        return apx * apx + apy * apy;
    }
    if (t > 1.0) {
        double bpx = x - seg.b.x;
        double bpy = y - seg.b.y;
        return bpx * bpx + bpy * bpy;
    }

    double projx = A.x + t * seg.dx;
    double projy = A.y + t * seg.dy;
    double dx = x - projx;
    double dy = y - projy;
    return dx * dx + dy * dy;
}

int QuadTree::add_segment(int x1, int y1, int x2, int y2, vector<vector<wstring>>& QueryPointData) {
    Segment seg = { Point{x1, y1}, Point{x2, y2},
             x2 - x1, y2 - y1,
             (long long)(x2 - x1) * (x2 - x1) + (long long)(y2 - y1) * (y2 - y1) };
    segments.push_back(seg);
    int seg_index = segments.size() - 1;
    insert_into_node(seg_index, 0, root);

    // 并行更新查询结果
#pragma omp parallel
    {
        vector<pair<int, double>> local_updates;

#pragma omp for
        for (size_t query_idx = 0; query_idx < query_points.size(); ++query_idx) {
            auto xy = query_points[query_idx];
            double new_dist_sq = calc_dist_sq(xy.first, xy.second, seg);
            auto& current_results = query_results[query_idx];

            if (current_results.empty() || new_dist_sq < current_results.back().second * current_results.back().second) {
                local_updates.emplace_back(query_idx, sqrt(new_dist_sq));
            }
        }

#pragma omp critical
        for (auto& update : local_updates) {
            int q_idx = update.first;
            double dist = update.second;
            for (auto& old_res : query_results[q_idx]) {
                seg_to_queries[old_res.first].erase(q_idx);
            }
            query_results[q_idx].clear();
            query_results[q_idx].emplace_back(seg_index + 1, dist);
            QueryPointData[q_idx + 1][1] = to_wstring(seg_index + 1);
            QueryPointData[q_idx + 1][2] = to_wstring(dist);
            seg_to_queries[seg_index + 1].insert(q_idx);
        }
    }




    return seg_index + 1;
}

void QuadTree::remove_segment(int seg_id, vector<vector<wstring>>& QueryPointData) {
    int seg_index = seg_id - 1;
    if (seg_index < 0 || static_cast<size_t>(seg_index) >= segments.size()) return;

    auto affected_queries = seg_to_queries[seg_id];
    seg_to_queries.erase(seg_id);

    for (auto node : seg_to_nodes[seg_index]) {
        auto it = find(node->segs.begin(), node->segs.end(), seg_index);
        if (it != node->segs.end()) {
            node->segs.erase(it);
        }
    }
    seg_to_nodes.erase(seg_index);

#pragma omp parallel for
    for (int query_idx : affected_queries) {
        auto xy = query_points[query_idx];
        auto new_res = query_internal(xy.first, xy.second, query_idx);
        QueryPointData[query_idx + 1][1] = to_wstring(new_res.first);
        QueryPointData[query_idx + 1][2] = to_wstring(new_res.second);

//#pragma omp critical
//        {
//            cout << "查询点 " << query_idx + 1 << " 更新结果: "
//                << "飞行线ID: " << new_res.first
//                << " 距离大小:" << new_res.second << "\n";
//        }
    }
}



pair<int, double> QuadTree::query_internal(int x, int y, int query_idx) {
    double min_dist_sq = DBL_MAX;
    int best_seg_id = -1;

    auto cmp = [](const pair<double, QuadNode*>& a, const pair<double, QuadNode*>& b) {
        return a.first > b.first;
        };
    priority_queue<pair<double, QuadNode*>, vector<pair<double, QuadNode*>>, decltype(cmp)> pq(cmp);
    pq.push({ 0, root });

    while (!pq.empty()) {
        double d_bound = pq.top().first;
        QuadNode* node = pq.top().second;
        pq.pop();

        if (d_bound >= min_dist_sq) continue;

#pragma omp parallel
        {
            double local_min = DBL_MAX;
            int local_id = -1;

#pragma omp for
            for (int seg_index : node->segs) {
                double d_sq = calc_dist_sq(x, y, segments[seg_index]);
                if (d_sq < local_min ||
                    (abs(d_sq - local_min) < 1e-9 && seg_index < local_id)) {
                    local_min = d_sq;
                    local_id = seg_index;
                }
            }

#pragma omp critical
            {
                if (local_min < min_dist_sq ||
                    (abs(local_min - min_dist_sq) < 1e-9 && local_id < best_seg_id)) {
                    min_dist_sq = local_min;
                    best_seg_id = local_id;
                }
            }
        }

        if (min_dist_sq == 0) break;

        if (!node->is_leaf) {
#pragma omp parallel for schedule(dynamic)
            for (auto child : node->children) {
                double d_child = calc_dist_rect_sq(x, y, child->rect);
                if (d_child < min_dist_sq) {
#pragma omp critical
                    pq.push({ d_child, child });
                }
            }
        }
    }

    pair<int, double> result(best_seg_id + 1, sqrt(min_dist_sq));

    if (query_idx != -1) {
#pragma omp critical
        {
            query_results[query_idx].push_back(result);
            seg_to_queries[result.first].insert(query_idx);
        }
    }

    return result;
}

pair<int, double> QuadTree::query(int x, int y) {
    int query_idx = query_points.size();
    query_points.emplace_back(x, y);
    return query_internal(x, y, query_idx);
}

void QuadTree::print_all_results() {
    cout << "当前所有查询结果:\n";
#pragma omp parallel for
    for (size_t i = 0; i < query_points.size(); ++i) {
        if (!query_results[i].empty()) {
            auto xy = query_points[i];
            auto res = query_results[i].back();
#pragma omp critical
            {
                cout << "查询点 " << i + 1 << " (" << xy.first << "," << xy.second << "): "
                    << "飞行线ID: " << res.first
                    << " 距离大小:" << res.second << "\n";
            }
        }
    }
}

void QuadTree::clear() {
    // 1. 删除根节点及其所有子节点（递归删除）
    delete root;

    // 2. 创建新的根节点（初始状态）
    root = new QuadNode(Rect{ 0, 0, 65535, 65535 });

    // 3. 清空所有存储的数据
    segments.clear();
    query_points.clear();
    query_results.clear();
    seg_to_nodes.clear();
    seg_to_queries.clear();
}